#include "klPrintf_USB.h"
#include <stdbool.h>
#include <inttypes.h>
#include <stdarg.h>
#include <util/delay.h>
#include <avr/pgmspace.h>
#include <util/atomic.h>

#include "CDCClassDevice.h"

void UART_Print (uint8_t AByte);
void UART_PrintAsHex (uint8_t AByte);
void UARTNewLine (void);
void UART_PrintUint16 (uint16_t);
void UART_PrintUint32 (uint32_t);
void UART_PrintInt (int16_t);
void UARTSendStringLen (const char *S, const uint8_t ALength);
void UARTSendString_P (const char *S);
void UARTSendArr (uint8_t *Arr, uint8_t ALength);
void UARTPrintArrAsHex (uint8_t *Arr, uint16_t ALength);

char UintToHexChar (uint8_t b);

// ************** Implementation ***********************
/*
 * c: character
 * i: signed int
 * u: unsigned int
 * s, S: string
 * X: hexadecimal, capital letters
 * A: hexadecimal array (arguments are &Array[0], ArrayLength)
 * (1...9) width - Minimum number of characters to be printed
 */
void klPrintf(const char *S, ...) {
    char c;
    bool WasPercent = false;
    va_list Arg;
    va_start(Arg, S);    // Set pointer to first argument
    while ((c = *S) != 0) {
        if (c == '%') {
            if (WasPercent) UART_Print(c);  // %%
            else {
                c = *(++S); // Get next symbol
                if (c == 'c') UART_Print((uint8_t)va_arg(Arg, int16_t));
                else if (c == 'u') UART_PrintUint16(va_arg(Arg, uint16_t));
                else if (c == 'U') UART_PrintUint32(va_arg(Arg, uint32_t));
                else if (c == 'i') UART_PrintInt(va_arg(Arg, int16_t));
                else if (c == 'X') UART_PrintAsHex((uint8_t)va_arg(Arg, uint16_t));
                else if ((c == 's') || (c == 'S')) CDC_Device_SendString(va_arg(Arg, char*));
                else if (c == 'A') UARTPrintArrAsHex(va_arg(Arg, uint8_t*), va_arg(Arg, uint16_t));
            }
            WasPercent = false;
        }
        else UART_Print(c);
        S++;
    } // while
    va_end(Arg);
}


void UART_Print (uint8_t AByte) {
    CDC_Device_SendByte(AByte);
}
void UART_PrintAsHex (uint8_t AByte){
    UART_Print(UintToHexChar (AByte >> 4));
    UART_Print(UintToHexChar (AByte & 0x0F));
}

char UintToHexChar (uint8_t b) {
    return ((b<=0x09) ? (b+'0') : (b+'A'-10));
}


// ============================== High-level ===================================
void UARTNewLine (void) {
    UART_Print ('\r');
}

// Send as digits
void UART_PrintUint16 (uint16_t ANumber) {
    uint8_t digit = '0';
    bool ShouldPrint = false;
    // >10000
    while (ANumber >= 10000) {
        digit++;
        ANumber -= 10000;
    }
    if (digit != '0'){
        UART_Print (digit);
        ShouldPrint = true;
    }
    digit = '0';
    // >1000
    while (ANumber >= 1000){
        digit++;
        ANumber -= 1000;
    }
    if ((digit != '0') || ShouldPrint) {
        UART_Print (digit);
        ShouldPrint = true;
    }
    digit = '0';

    // > 100
    while (ANumber >= 100) {        // Still larger than 100 ?
        digit++;            // Increment first digit
        ANumber -= 100;
    }
    if ((digit != '0') || ShouldPrint) {
        UART_Print (digit);
        ShouldPrint = true;
    }
    digit = '0';
    // > 10
    while (ANumber >= 10) {     // Still larger than 10 ?
        digit++;            // Increment second digit
        ANumber -= 10;
    }
    UART_Print (digit);
    UART_Print ('0'+ANumber);
}

void UART_PrintUint32 (uint32_t ANumber) {
    uint8_t digit = '0';
    // >1000000000
        while (ANumber >= 1000000000) {
            digit++;
            ANumber -= 1000000000;
        }
        UART_Print (digit);
        digit = '0';
    // >100000000
        while (ANumber >= 100000000) {
            digit++;
            ANumber -= 100000000;
        }
        UART_Print (digit);
        digit = '0';
    // >10000000
        while (ANumber >= 10000000) {
            digit++;
            ANumber -= 10000000;
        }
        UART_Print (digit);
        digit = '0';
    // >1000000
        while (ANumber >= 1000000) {
            digit++;
            ANumber -= 1000000;
        }
        UART_Print (digit);
        digit = '0';
    // >100000
        while (ANumber >= 100000) {
            digit++;
            ANumber -= 100000;
        }
        UART_Print (digit);
        digit = '0';
    // >10000
    while (ANumber >= 10000) {
        digit++;
        ANumber -= 10000;
    }
    UART_Print (digit);
    digit = '0';
    // >1000
    while (ANumber >= 1000){
        digit++;
        ANumber -= 1000;
    }
    UART_Print (digit);
    digit = '0';

    // > 100
    while (ANumber >= 100) {        // Still larger than 100 ?
        digit++;            // Increment first digit
        ANumber -= 100;
    }
    UART_Print (digit);
    digit = '0';
    // > 10
    while (ANumber >= 10) {     // Still larger than 10 ?
        digit++;            // Increment second digit
        ANumber -= 10;
    }
    UART_Print (digit);
    UART_Print ('0'+ANumber);
}
void UART_PrintInt (int16_t ANumber) {
    if (ANumber < 0) {
        UART_Print ('-');
        ANumber = -ANumber;
    }
    else UART_Print (' ');
    UART_PrintUint16 (ANumber);
}

void UARTSendStringLen (const char *S, const uint8_t ALength) {
    for (uint8_t i=0; i<ALength; i++) UART_Print (*S++);
}
void UARTSendString_P (const char *S) {
    uint8_t FChar;
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
        while ((FChar = pgm_read_byte(S++)) != '\0') UART_Print (FChar);
    }
}

void UARTSendArr (uint8_t *Arr, uint8_t ALength) {
    while (ALength--) UART_Print(*(Arr++));
}

void UARTPrintArrAsHex (uint8_t *Arr, uint16_t ALength) {
    while (ALength--) {
        UART_PrintAsHex(*(Arr++));
        UART_Print (' ');
    }
}
